#include <iostream>
#include <stdio.h>
#include <cmath>
#include "GpsWrapper.h"
#include "boost/thread.hpp"


using namespace std;

GpsWrapper::GpsWrapper(){//Initialise that sucker
    d = odo = 0;
    time(&time0);
    time(&time1);
    velocity = 0;
    totalTime = 0.0f;
}

float * GpsWrapper::GetSquareLatLong(float gpsBuffer[2]){  //Populate the referenced array with a simulated
                                                            //GPS feed for our known Square test route

    update();//Derive a relative odometer reading according to an expression for velocity

    switch ((d/400)%4){ //'Wrap' the counter reading around a 400m square, and interpolate
        case 0:         //the GPS coordinates given that the route topology is pre-determined
        gpsBuffer[1] = 0.01695 - (((0.01695 - 0.01137)/400) * d);
        gpsBuffer[0] = 50.86338;
        break;
        case 1:
        gpsBuffer[1] = 0.01137;
        gpsBuffer[0] = 50.86338 - (((50.86338 - 50.85983)/400) * (d - 400));
        break;
        case 2:
        gpsBuffer[1] = 0.01137 + (((0.01695 - 0.01137)/400) * (d - 800));
        gpsBuffer[0] = 50.85983;
        break;
        case 3:
        gpsBuffer[1] = 0.01695;
        gpsBuffer[0] = 50.85983 + (((50.86338 - 50.85983)/400) * (d - 1200));
        break;
        default:
        break;

    }

    return gpsBuffer;
}

void GpsWrapper::update(){//private method for updating 'Hardware' according to velocity
    boost::mutex mutex;
    boost::mutex::scoped_lock lock(mutex);


    time(&time0);

    totalTime += difftime(time0,time1);
    velocity = 6.0f*(sin(totalTime/15.0f)) + 20;    //Velocity is given as a function of time,
               //((-totalTime)/25) + 30;                                  //In this case, a sine wave

    d = odo += (difftime(time0,time1) * velocity);
    d %= 1600;

    time(&time1);

}
